#!/usr/bin/env python3

import rospy
import time
from std_msgs.msg import String
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import Trajectory
from sensor_msgs.msg import LaserScan


def state_callback(state_msg):
    global mavproxy_state
    mavproxy_state = state_msg

def set_position():
        pose_msg=PoseStamped()
        pose_msg.header.stamp=rospy.Time.now()
        pose_msg.pose.position.x=5
        pose_msg.pose.position.y=5
        pose_msg.pose.position.z=5
        return pose_msg

if __name__ == '__main__':  
    try:
        rospy.init_node('px4_avoidance_handler', anonymous=True)
        mavros_pos_setpoint_pub_ = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
        pose_msg=set_position()
        # mavros_pos_setpoint_pub_ \ mavros_obstacle_distance_pub_
        while True:
            rospy.Rate(0.5)
            mavros_pos_setpoint_pub_.publish(pose_msg)

        rospy.spin()
    except rospy.ROSInterruptException:
        pass
